#include "Render.h"

using namespace qglviewer;
using namespace std;

void setTransform (const double pos[3], const double rot[12]);
void drawPatch (float p1[3], float p2[3], float p3[3], int level);

void CameraPathPlayer::checkIfGrabsMouse(int x, int y, const Camera* const)
{
	// Rectangular activation array - May have to be tune depending on your default font size
	setGrabsMouse((x < 80) && (y<yPos()) && ((yPos()-y) < 16));
}

void Render::displayPlayers()
{
	for (int i=0; i<nbPlayers_; ++i)
	{
		CameraPathPlayer* cpp = player_[i];
		if (cpp)
		{
			QString s;
			if (cpp->grabsMouse())
			{
				glColor3f(1,1,1);
				if (camera()->keyFrameInterpolator(i)->numberOfKeyFrames() > 1)
					s = "Play path F" + QString::number(i);
				else
					s = "Restore pos F" + QString::number(i);
			}
			else
			{
				glColor3f(0.6f, 0.6f, 0.6f);
				if (camera()->keyFrameInterpolator(i)->numberOfKeyFrames() > 1)
					s = "Path F" + QString::number(i);
				else
					s = "Pos F" + QString::number(i);
			}
			drawText(10, cpp->yPos()-3, s);
		}
	}
}

void Render::updatePlayers()
{
	for (int i=0; i<nbPlayers_; ++i)
	{
		// Check if CameraPathPlayer is still valid
		if ((player_[i]) && (!camera()->keyFrameInterpolator(i)))
		{
			delete player_[i];
			player_[i] = NULL;
		}
		// Or add it if needed
		if ((camera()->keyFrameInterpolator(i)) && (!player_[i]))
			player_[i] = new CameraPathPlayer(i);
	}
}

void Render::init()
{ 
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	// Light0 is the default ambient light
	glEnable(GL_LIGHT0);

	//glEnable(GL_LIGHT1);
	//const GLfloat light_ambient[4]  = {0.8f, 0.2f, 0.2f, 1.0};
	//const GLfloat light_diffuse[4]  = {1.0, 0.4f, 0.4f, 1.0};
	//const GLfloat light_specular[4] = {1.0, 0.0, 0.0, 1.0};

	//glLightf( GL_LIGHT1, GL_SPOT_EXPONENT, 3.0);
	//glLightf( GL_LIGHT1, GL_SPOT_CUTOFF, 20.0);
	//glLightf( GL_LIGHT1, GL_CONSTANT_ATTENUATION, 0.5);
	//glLightf( GL_LIGHT1, GL_LINEAR_ATTENUATION, 1.0);
	//glLightf( GL_LIGHT1, GL_QUADRATIC_ATTENUATION, 1.5);
	//glLightfv(GL_LIGHT1, GL_AMBIENT,  light_ambient);
	//glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
	//glLightfv(GL_LIGHT1, GL_DIFFUSE,  light_diffuse);

	//light1 = new ManipulatedFrame();

	//light1->setPosition(-1.0, 0, 1.0);
	//// Align z axis with -position direction : look at scene center
	//light1->setOrientation(Quaternion(Vec(0,0,1), -light1->position()));


	//glEnable(GL_LIGHT2);
	//const GLfloat light_ambient2[4]  = {0.5f, 0.5f, 2.0, 1.0};
	//const GLfloat light_diffuse2[4]  = {0.8f, 0.8f, 1.0, 1.0};
	//const GLfloat light_specular2[4] = {0.0, 0.0, 1.0, 1.0};

	//glLightfv(GL_LIGHT2, GL_AMBIENT,  light_ambient2);
	//glLightfv(GL_LIGHT2, GL_SPECULAR, light_specular2);
	//glLightfv(GL_LIGHT2, GL_DIFFUSE,  light_diffuse2);

	//light2 = new ManipulatedFrame();

	//light2->setPosition(-1.0, 0, 1.0);

	// Absolutely needed for MouseGrabber
	setMouseTracking(true);

	// In order to make the manipulatedFrame displacements clearer
	// Initialize the CameraPathPlayer MouseGrabber array
	nbPlayers_ = 12;
	player_ = new CameraPathPlayer*[nbPlayers_];
	for (int i=0; i<nbPlayers_; ++i)
		player_[i] = NULL;

	reset();

	restoreStateFromFile();

	setForegroundColor(QColor(0.95 * 255, 0.88 * 255, 0.1 * 255));
	setBackgroundColor(QColor(0.75 * 255, 0.75 * 255, 0.75 * 255));
	setTextures();
	setFPSIsDisplayed(true);
	setAxisIsDrawn(true);
	setAnimationPeriod(20);
	startAnimation();
}

void Render::reset()
{
	phyBodies.clear();
	Macro::s_mousePos = Vector3d(dBodyGetPosition(pw->endEffector));

	for(map<dGeomID, dBodyID>::iterator iter = pw->geoms.begin(); iter != pw->geoms.end(); iter++)
	{
		dGeomID geomID = iter->first;
		dBodyID bodyID = iter->second;

		const dReal* pos = dGeomGetPosition(geomID);
		dQuaternion rot;
		dGeomGetQuaternion(geomID, rot);

		PhyBody pb;
		pb.mf.SetPhysicsWorld(pw, geomID);
		pb.mf.setPositionAndOrientation(Vec(pos[0], pos[1], pos[2]), qglviewer::Quaternion(rot[1], rot[2], rot[3], rot[0]));
		pb.mf.resetMouseState();
		pb.bodyID = bodyID;
		pb.geomID = geomID;
		pb.pos = dGeomGetPosition(geomID);
		pb.rot = dGeomGetRotation(geomID);

		phyBodies.push_back(pb);
	}
}

void Render::drawConForce()
{
	if(!pw->conInfo.status)
		return;

	Vector3d conForce = pw->conInfo.force;
	Vector3d conPos = pw->conInfo.pos;

	Vec forceFrom(conPos);
	Vec forceTo(conPos + conForce/40);

	float color[4] = {0.5, 0.4, 0.9, 0.2};
	glDisable (GL_TEXTURE_2D);
	if (color[3] < 1) 
	{
		glEnable (GL_BLEND);
		glBlendFunc (GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	}
	else
	{
		glDisable (GL_BLEND);
	}
	glEnable (GL_NORMALIZE);
	glShadeModel (GL_SMOOTH);
	glColor4f(color[0], color[1], color[2], color[3]);
	drawArrow(forceFrom, forceTo);

	color[0] = 0.95; color[1] = 0.88; color[2] = 0.1; color[3] = 1.0;
	dVector3 pos = {conPos[0], conPos[1], conPos[2]};
	drawJoint(color, 0.015, pos);

	Vec screenPos = camera()->projectedCoordinatesOf(Vec(conPos + conForce/40));
	double forceLength = conForce.norm();
	drawText((int)screenPos.x, (int)screenPos.y, QString::number(forceLength) + "N" + " (" + QString::number(conForce[0]) + ", " + QString::number(conForce[1]) + ", " + QString::number(conForce[2]) + ")", QFont("Times", 14));
}

void Render::drawJoint(float color[4], double radius, dReal* pos)
{
	glDisable (GL_TEXTURE_2D);
	if (color[3] < 1) 
	{
		glEnable (GL_BLEND);
		glBlendFunc (GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	}
	else
	{
		glDisable (GL_BLEND);
	}
	glEnable (GL_NORMALIZE);
	glShadeModel (GL_SMOOTH);
	glColor4f(color[0], color[1], color[2], color[3]);

	double rot[12] = {0.0};
	rot[0] = 1.0;
	rot[5] = 1.0;
	rot[10] = 1.0;
	setTransform(pos, rot);

	glScaled (radius,radius,radius);
	static int sphere_quality = 2;

	GLfloat idata[12][3] = {
		{-ICX, 0, ICZ},
		{ICX, 0, ICZ},
		{-ICX, 0, -ICZ},
		{ICX, 0, -ICZ},
		{0, ICZ, ICX},
		{0, ICZ, -ICX},
		{0, -ICZ, ICX},
		{0, -ICZ, -ICX},
		{ICZ, ICX, 0},
		{-ICZ, ICX, 0},
		{ICZ, -ICX, 0},
		{-ICZ, -ICX, 0}
	};

	int index[20][3] = {
		{0, 4, 1},	  {0, 9, 4},
		{9, 5, 4},	  {4, 5, 8},
		{4, 8, 1},	  {8, 10, 1},
		{8, 3, 10},   {5, 3, 8},
		{5, 2, 3},	  {2, 7, 3},
		{7, 10, 3},   {7, 6, 10},
		{7, 11, 6},   {11, 0, 6},
		{0, 1, 6},	  {6, 1, 10},
		{9, 0, 11},   {9, 11, 2},
		{9, 2, 5},	  {7, 2, 11},
	};

	static GLuint listnum = 0;
	if (listnum==0) {
		listnum = glGenLists (1);
		glNewList (listnum,GL_COMPILE);
		glBegin (GL_TRIANGLES);
		for (int i=0; i<20; i++) {
			drawPatch (&idata[index[i][2]][0],&idata[index[i][1]][0],
				&idata[index[i][0]][0],sphere_quality);
		}
		glEnd();
		glEndList();
	}
	glCallList (listnum);

	glPopMatrix();
	glDisable (GL_NORMALIZE);
}

void Render::checkReplay()
{
	if(Macro::s_replay)
	{
		static int massCount = 0;
		static int muCount = 0;
		static int replayIterCount = 0;
		static int replayTimeStepCount = 0;

		if(muCount < 4)
		{
			if(massCount < 4)
			{
				if(replayIterCount < Macro::s_replayIters)
				{
					Vector3d initPos(traj->pathPoints.at(0).mousePos);
					Vector3d realPos(dBodyGetPosition(pw->endEffector));

					if((realPos - initPos).norm() > POS_THRESH && replayTimeStepCount == 0)
					{
						Macro::s_gain = 2.0;
						Macro::s_maxForce = 300.0;
						Macro::s_record = false;
						Macro::s_mousePos = initPos;
					}

					else if(replayTimeStepCount < traj->pathPoints.size())
					{
						Macro::s_gain = 5.0;
						Macro::s_maxForce = 500.0;
						Macro::s_record = true;
						Macro::s_mousePos = Vector3d(traj->pathPoints.at(replayTimeStepCount).mousePos);
						replayTimeStepCount += (int) (0.02 / pw->timeStep);
					}

					else if(replayTimeStepCount == traj->pathPoints.size())
					{
						dMass m;
						dBodyGetMass(pw->magicBoxBody, &m);
						double totalMass = m.mass;
						//Vector3d pos(dBodyGetPosition(pw->magicBoxBody));
						//Eigen::Quaternion<double> quat(dBodyGetQuaternion(pw->magicBoxBody));

						pw->reset();
						ccd->reset();
						model->loadModel("data/ball.xml");
						reset();

						//dBodySetPosition(pw->magicBoxBody, pos[0], pos[1], pos[2]);
						//dQuaternion q = {quat.w(), quat.x(), quat.y(), quat.z()};
						//dBodySetQuaternion(pw->magicBoxBody, q);
						dMassAdjust(&m, totalMass);
						dBodySetMass(pw->magicBoxBody, &m);


						QString storePath = "./stats/" + traj->name;
						if (!QDir(storePath).exists()) {
							QDir().mkdir(storePath);
						}

						storePath = storePath + "/data";
						if (!QDir(storePath).exists()) {
							QDir().mkdir(storePath);
						}

						storePath = storePath + "/" + "M_" + QString::number(totalMass, 'f', 3) + "_F_" + QString::number(Macro::s_mu2, 'f', 3);
						if(!QDir(storePath).exists()){
							QDir().mkdir(storePath);
						}

						storePath = storePath + "/" + QString::number(replayIterCount) + ".csv";
						traj->saveT(storePath);

						traj->totalInfos.clear();
						Macro::s_timeStep = 0.0;
						replayIterCount++;
						replayTimeStepCount = 0;
						Macro::s_record = false;
					}
				}
				else
				{
					replayIterCount = 0;
					massCount++;
					dMass m;
					dBodyGetMass(pw->magicBoxBody, &m);
					dMassAdjust(&m, m.mass + 0.05);
					dBodySetMass(pw->magicBoxBody, &m);
				}
			}
			else
			{
				dMass m;
				dBodyGetMass(pw->magicBoxBody, &m);
				dMassAdjust(&m, m.mass - 4 * 0.05);
				dBodySetMass(pw->magicBoxBody, &m);

				massCount = 0;
				muCount++;
				Macro::s_mu2 += 0.2;
			}
		}
		else
		{
			Macro::s_replay = false;
			Macro::s_record = false;
			replayIterCount = 0;
			replayTimeStepCount = 0;
			massCount = 0;
			muCount = 0;
		}
	}
}

void Render::paintEvent(QPaintEvent *event)
{
	//Q_UNUSED(event)
	//QPainter painter;
	//painter.begin(this);
	//// Save current OpenGL state
	//glMatrixMode(GL_PROJECTION);
	//glPushMatrix();
	//glMatrixMode(GL_MODELVIEW);
	//glPushMatrix();

	//qglClearColor(backgroundColor());
	//preDraw();
	//draw();
	//postDraw();

	////Restore OpenGL state
	//glMatrixMode(GL_MODELVIEW);
	//glPopMatrix();
	//glMatrixMode(GL_PROJECTION);
	//glPopMatrix();

	//painter.save();
	//glColor4f(0.1, 0.1, 0.1, 1);
	//painter.drawEllipse(QPoint(500, 500), 5, 5);

	//painter.restore();
	//	//update();
	//painter.end();
}

void Render::draw()
{
	qglClearColor(backgroundColor());
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	//saveStateToFile();

	glEnable (GL_LIGHTING);

	float pos[4] = {1.0, 0.0, 1.0, 0.0};
	// Directionnal light
	glLightfv(GL_LIGHT0, GL_POSITION, pos);

	//pos[3] = 1.0;
	//// Spot light
	////light1->getPosition(pos[0], pos[1], pos[2]);
	////glLightfv(GL_LIGHT1, GL_POSITION, pos);
	////glLightfv(GL_LIGHT1, GL_SPOT_DIRECTION, light1->inverseTransformOf(Vec(0,0,1)));

	////if (light1->grabsMouse())
	////	drawLight(GL_LIGHT1, 1.2f);
	////else
	////	drawLight(GL_LIGHT1);

	//light2->getPosition(pos[0], pos[1], pos[2]);
	//glLightfv(GL_LIGHT2, GL_POSITION, pos);

	//if (light2->grabsMouse())
	//	drawLight(GL_LIGHT2, 1.2f);
	//else
	//	drawLight(GL_LIGHT2);

	drawGround();
	drawConForce();
	//drawState();

	for (map<QString, dJointID>::iterator iter = pw->armJoints.begin(); iter != pw->armJoints.end(); iter++)
	{
		dJointID jointID = iter->second;
		dVector3 result;
		float jointColor[4] = {0.95, 0.88, 0.1, 0.5};

		//reset joint param

		dJointSetHingeParam(jointID, dParamFMax, 0.0);
		dJointSetHingeParam(jointID, dParamVel, 0.0);

		dJointGetHingeAnchor(jointID, result);
		drawJoint(jointColor, 0.015, result);
	}

	for (QList<PhyBody>::iterator iter = phyBodies.begin(); iter != phyBodies.end(); iter++)
	{
		float color[4] = {0.75, 0.75, 0.75, 1.0};

		dBodyID bodyID = (*iter).bodyID;
		dGeomID geomID = (*iter).geomID;

		if(bodyID != NULL)
		{
			if((*iter).mf.selectionMode == MyManipulatedFrame::SELECT)
			{
				color[0] = 0.8;
				color[1] = 0.5;
				color[2] = 0.5;
				color[3] = 0.9;
			}

			if ((*iter).mf.grabsMouse())
			{
				color[0] = 0.5;
				color[1] = 0.8;
				color[2] = 0.5;
				color[3] = 0.9;

				if((*iter).mf.isPress() && !(*iter).mf.isRelease())
				{
					//if(pw->mouseJoint == NULL && pw->mouseBody1 == NULL && pw->mouseBody2 == NULL)
					//{
					//	pw->mouseBody1 = dBodyCreate(pw->world);
					//	dBodySetKinematic(pw->mouseBody1);

					//	pw->mouseBody2 = bodyID;

					//	float x, y, z;
					//	(*iter).mf.getPosition(x, y, z);
					//	dBodySetPosition(pw->mouseBody1, x, y, z);

					//	double q[4];
					//	(*iter).mf.getOrientation(q[1], q[2], q[3], q[0]);		
					//	dBodySetQuaternion(pw->mouseBody1, q);

					//	pw->mouseJoint = dJointCreateFixed (pw->world,pw->mousegroup);

					//	dReal cfm;  // 0.2 default 1e-10 ~ 1
					//	dReal erp;  // 0.1 default 0.1 ~ 0.8
					//	dReal kp = 20.0;
					//	dReal kd = 2.0;
					//	dReal h = pw->timeStep;
					//	
					//	cfm = 1 / (h * kp + kd);  //0.8
					//	erp = h * kp / (h * kp + kd); //0.2

					//	dJointSetFixedParam(pw->mouseJoint, dParamERP, erp);
					//	dJointSetFixedParam(pw->mouseJoint, dParamCFM, cfm);
					//	
					//	dJointAttach(pw->mouseJoint, pw->mouseBody1, pw->mouseBody2);
					//	dJointSetFixed(pw->mouseJoint);

					//	dBodySetGravityMode(pw->mouseBody1, 0);
					//	dBodySetGravityMode(pw->mouseBody2, 0);
					//	
					//	pw->mouseControl = true;
					//}

					float x, y, z;
					(*iter).mf.getPosition(x, y, z);
					//dBodySetPosition(pw->mouseBody1, x, y, z);

					double q[4];
					(*iter).mf.getOrientation(q[1], q[2], q[3], q[0]);		
					//dBodySetQuaternion(pw->mouseBody1, q);

					Macro::s_mousePos = Vector3d(x, y ,z);
					Macro::s_mouseControl = true;
				}
				else if(!(*iter).mf.isPress() && (*iter).mf.isRelease())
				{
					(*iter).mf.mySetGrabsMouse(false);
					(*iter).mf.resetMouseState();
					if(pw->mouseJoint != NULL)
					{
						dJointGroupEmpty(pw->mousegroup);
						pw->mouseJoint = NULL;
					}
					if(pw->mouseBody1 != NULL)
					{
						dBodyDestroy(pw->mouseBody1);
						pw->mouseBody1 = NULL;
					}
					if(pw->mouseBody2 != NULL)
					{
						dBodySetLinearVel(pw->mouseBody2, 0.0, 0.0, 0.0);
						dBodySetAngularVel(pw->mouseBody2, 0.0, 0.0, 0.0);
						pw->mouseBody2 = NULL;
					}
					Macro::s_mouseControl = false;
				}
			}

			if(!(*iter).mf.grabsMouse())
			{
				//dBodySetGravityMode(bodyID, 0);
				const dReal* pos = dGeomGetPosition(geomID);
				dQuaternion rot;
				dGeomGetQuaternion(geomID, rot);
				(*iter).mf.setPositionAndOrientation(Vec(pos[0], pos[1], pos[2]), qglviewer::Quaternion(rot[1], rot[2], rot[3], rot[0]));
			}
		}

		(*iter).pos = dGeomGetPosition(geomID);
		(*iter).rot = dGeomGetRotation(geomID);

		(*iter).draw(color);
	}

	//updatePlayers();
	//glDisable(GL_LIGHTING);
	//displayPlayers();
	//glEnable(GL_LIGHTING);

	checkReplay();

	ccd -> updateCCD(pw);
	ccd -> computeCCD(Macro::s_mousePos);

	pw -> animatePhysics(ccd->joints);
}

QString Render::helpString() const
{
	QString text("<h2>M o u s e G r a b b e r </h2>");
	text += "This example illustrates the use of <i>MouseGrabber</i>, which is an abstract ";
	text += "class for objects that react (usually when the mouse hovers over them).<br><br>";
	text += "Define new camera paths (or positions) using <b>Alt</b>+[<b>F1</b>-<b>F12</b>]. ";
	text += "New <i>MouseGrabbers</i> are then created and displayed in the upper left corner. ";
	text += "Note how they react when the mouse hovers, and click them to play the associated path.<br><br>";
	text += "<i>ManipulatedFrame</i>, such as the ones which define the spirals' positions, are ";
	text += "also <i>MouseGrabbers</i>. When the mouse is close to the spiral center, the <i>ManipulatedFrame</i> ";
	text += "will grab to mouse click (as if the <b>Control</b> key was pressed). This is very convenient ";
	text += "to intuitively move scene objects (such as lights) without any key or GUI interaction.<br><br>";
	text += "Note that <code>setMouseTracking()</code> must be enabled to use <i>MouseGrabbers</i>.";
	return text;
}

void Render::drawGround()
{
	//glDisable (GL_LIGHTING);
	glShadeModel (GL_FLAT);
	glEnable (GL_DEPTH_TEST);
	glDepthFunc (GL_LESS);
	// Enable GL textures
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glEnable(GL_TEXTURE_2D);
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
	ground_texture->bind (0);

	const float gsize = 100.0f;
	const float offset = 0; // -0.001f; ... polygon offsetting doesn't work well
	const float ground_scale = 1.0f/1.0f;	// ground texture scale (1/size)
	const float ground_ofsx = 0.5;		// offset of ground texture
	const float ground_ofsy = 0.5;

	glBegin (GL_QUADS);
	glNormal3f (0,0,1);
	glTexCoord2f (-gsize*ground_scale + ground_ofsx,
		-gsize*ground_scale + ground_ofsy);
	glVertex3f (-gsize,-gsize,offset);
	glTexCoord2f (gsize*ground_scale + ground_ofsx,
		-gsize*ground_scale + ground_ofsy);
	glVertex3f (gsize,-gsize,offset);
	glTexCoord2f (gsize*ground_scale + ground_ofsx,
		gsize*ground_scale + ground_ofsy);
	glVertex3f (gsize,gsize,offset);
	glTexCoord2f (-gsize*ground_scale + ground_ofsx,
		gsize*ground_scale + ground_ofsy);
	glVertex3f (-gsize,gsize,offset);
	glEnd();

	//glDisable (GL_FOG);
}

void Render::setTextures()
{
	texture[DS_SKY] = sky_texture = new Texture("textures/sky.ppm");
	texture[DS_GROUND] = ground_texture = new Texture("textures/ground.ppm");
	texture[DS_WOOD] = wood_texture = new Texture("textures/wood.ppm");
	texture[DS_CHECKERED] = checkered_texture = new Texture ("textures/checkered.ppm");
}

void PhyBody::draw(const float color[4])
{
	dGeomID geomID = this->geomID;
	int geomType = dGeomGetClass(geomID);

	if(geomType == dSphereClass)
	{
		dReal radius = dGeomSphereGetRadius(geomID);
		setupDrawingMode(color);

		drawSphere(radius, color);
	}
	else if(geomType == dCylinderClass)
	{
		dReal radius, length;
		dGeomCylinderGetParams(geomID, &radius, &length);
		setupDrawingMode(color);

		drawCylinder(length, radius, 0.0, color);
	}
	else if(geomType == dCapsuleClass)
	{
		dReal radius, length;
		dGeomCapsuleGetParams(geomID, &radius, &length);
		setupDrawingMode(color);

		drawCapsule(length, radius, color);
	}

	else if(geomType == dBoxClass)
	{
		dVector3 result;
		float sides[3];
		dGeomBoxGetLengths(geomID, result);
		for(int i = 0; i < 3; i++)
			sides[i] = result[i];
		setupDrawingMode(color);

		drawBox(sides, color);
	}
}

void PhyBody::drawCylinder (float l, float r, float zoffset, const float color[4])
{
	glShadeModel (GL_SMOOTH);
	glColor4f(color[0], color[1], color[2], color[3]);
	//glPushMatrix();
	//glMultMatrixd(mf.matrix());
	setTransform(pos, rot);

	int i;
	float tmp,ny,nz,a,ca,sa;
	const int n = 24;	// number of sides to the cylinder (divisible by 4)

	l *= 0.5;
	a = float(M_PI*2.0)/float(n);
	sa = (float) sin(a);
	ca = (float) cos(a);

	// draw cylinder body
	ny=1; nz=0;		  // normal vector = (0,ny,nz)
	glBegin (GL_TRIANGLE_STRIP);
	for (i=0; i<=n; i++) {
		glNormal3d (ny,nz,0);
		glVertex3d (ny*r,nz*r,l+zoffset);
		glNormal3d (ny,nz,0);
		glVertex3d (ny*r,nz*r,-l+zoffset);
		// rotate ny,nz
		tmp = ca*ny - sa*nz;
		nz = sa*ny + ca*nz;
		ny = tmp;
	}
	glEnd();

	// draw top cap
	glShadeModel (GL_FLAT);
	ny=1; nz=0;		  // normal vector = (0,ny,nz)
	glBegin (GL_TRIANGLE_FAN);
	glNormal3d (0,0,1);
	glVertex3d (0,0,l+zoffset);
	for (i=0; i<=n; i++) {
		if (i==1 || i==n/2+1)
			setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
		glNormal3d (0,0,1);
		glVertex3d (ny*r,nz*r,l+zoffset);
		if (i==1 || i==n/2+1)
			setColor (color[0],color[1],color[2],color[3]);

		// rotate ny,nz
		tmp = ca*ny - sa*nz;
		nz = sa*ny + ca*nz;
		ny = tmp;
	}
	glEnd();

	// draw bottom cap
	ny=1; nz=0;		  // normal vector = (0,ny,nz)
	glBegin (GL_TRIANGLE_FAN);
	glNormal3d (0,0,-1);
	glVertex3d (0,0,-l+zoffset);
	for (i=0; i<=n; i++) {
		if (i==1 || i==n/2+1)
			setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
		glNormal3d (0,0,-1);
		glVertex3d (ny*r,nz*r,-l+zoffset);
		if (i==1 || i==n/2+1)
			setColor (color[0],color[1],color[2],color[3]);

		// rotate ny,nz
		tmp = ca*ny + sa*nz;
		nz = -sa*ny + ca*nz;
		ny = tmp;
	}
	glEnd();

	glPopMatrix();
}

void PhyBody::drawSphere(float radius, const float color[4])
{
	glEnable (GL_NORMALIZE);
	glShadeModel (GL_SMOOTH);
	glColor4f(color[0], color[1], color[2], color[3]);
	//glPushMatrix();
	//glMultMatrixd(mf.matrix());

	setTransform(pos, rot);

	glScaled (radius,radius,radius);
	static int sphere_quality = 3;

	GLfloat idata[12][3] = {
		{-ICX, 0, ICZ},
		{ICX, 0, ICZ},
		{-ICX, 0, -ICZ},
		{ICX, 0, -ICZ},
		{0, ICZ, ICX},
		{0, ICZ, -ICX},
		{0, -ICZ, ICX},
		{0, -ICZ, -ICX},
		{ICZ, ICX, 0},
		{-ICZ, ICX, 0},
		{ICZ, -ICX, 0},
		{-ICZ, -ICX, 0}
	};

	int index[20][3] = {
		{0, 4, 1},	  {0, 9, 4},
		{9, 5, 4},	  {4, 5, 8},
		{4, 8, 1},	  {8, 10, 1},
		{8, 3, 10},   {5, 3, 8},
		{5, 2, 3},	  {2, 7, 3},
		{7, 10, 3},   {7, 6, 10},
		{7, 11, 6},   {11, 0, 6},
		{0, 1, 6},	  {6, 1, 10},
		{9, 0, 11},   {9, 11, 2},
		{9, 2, 5},	  {7, 2, 11},
	};

	static GLuint listnum = 0;
	if (listnum==0) {
		listnum = glGenLists (1);
		glNewList (listnum,GL_COMPILE);
		glBegin (GL_TRIANGLES);
		for (int i=0; i<20; i++) {
			drawPatch (&idata[index[i][2]][0],&idata[index[i][1]][0],
				&idata[index[i][0]][0],sphere_quality);
		}
		glEnd();
		glEndList();
	}
	glCallList (listnum);

	glPopMatrix();
	glDisable (GL_NORMALIZE);
}

void PhyBody::drawCapsule (float l, float r, const float color[4])
{
	glShadeModel (GL_SMOOTH);

	//glPushMatrix();
	//glMultMatrixd(mf.matrix());
	glColor4f(color[0], color[1], color[2], color[3]);
	setTransform(pos, rot);

	int i,j;
	int capped_cylinder_quality = 3;
	float tmp,nx,ny,nz,start_nx,start_ny,a,ca,sa;
	// number of sides to the cylinder (divisible by 4):
	const int n = capped_cylinder_quality*4;

	l *= 0.5;
	a = float(M_PI*2.0)/float(n);
	sa = (float) sin(a);
	ca = (float) cos(a);

	// draw cylinder body
	ny=1; nz=0;		  // normal vector = (0,ny,nz)
	glBegin (GL_TRIANGLE_STRIP);
	for (i=0; i<=n; i++) {
		glNormal3d (ny,nz,0);
		glVertex3d (ny*r,nz*r,l);
		glNormal3d (ny,nz,0);
		glVertex3d (ny*r,nz*r,-l);
		// rotate ny,nz
		tmp = ca*ny - sa*nz;
		nz = sa*ny + ca*nz;
		ny = tmp;
	}
	glEnd();

	// draw first cylinder cap
	start_nx = 0;
	start_ny = 1;
	for (j=0; j<(n/4); j++) {
		// get start_n2 = rotated start_n
		float start_nx2 =  ca*start_nx + sa*start_ny;
		float start_ny2 = -sa*start_nx + ca*start_ny;
		// get n=start_n and n2=start_n2
		nx = start_nx; ny = start_ny; nz = 0;
		float nx2 = start_nx2, ny2 = start_ny2, nz2 = 0;
		glBegin (GL_TRIANGLE_STRIP);
		for (i=0; i<=n; i++) {
			glNormal3d (ny2,nz2,nx2);
			glVertex3d (ny2*r,nz2*r,l+nx2*r);
			glNormal3d (ny,nz,nx);
			glVertex3d (ny*r,nz*r,l+nx*r);
			// rotate n,n2
			tmp = ca*ny - sa*nz;
			nz = sa*ny + ca*nz;
			ny = tmp;
			tmp = ca*ny2- sa*nz2;
			nz2 = sa*ny2 + ca*nz2;
			ny2 = tmp;
		}
		glEnd();
		start_nx = start_nx2;
		start_ny = start_ny2;
	}

	// draw second cylinder cap
	start_nx = 0;
	start_ny = 1;
	for (j=0; j<(n/4); j++) {
		// get start_n2 = rotated start_n
		float start_nx2 = ca*start_nx - sa*start_ny;
		float start_ny2 = sa*start_nx + ca*start_ny;
		// get n=start_n and n2=start_n2
		nx = start_nx; ny = start_ny; nz = 0;
		float nx2 = start_nx2, ny2 = start_ny2, nz2 = 0;
		glBegin (GL_TRIANGLE_STRIP);
		for (i=0; i<=n; i++) {
			glNormal3d (ny,nz,nx);
			glVertex3d (ny*r,nz*r,-l+nx*r);
			glNormal3d (ny2,nz2,nx2);
			glVertex3d (ny2*r,nz2*r,-l+nx2*r);
			// rotate n,n2
			tmp = ca*ny - sa*nz;
			nz = sa*ny + ca*nz;
			ny = tmp;
			tmp = ca*ny2- sa*nz2;
			nz2 = sa*ny2 + ca*nz2;
			ny2 = tmp;
		}
		glEnd();
		start_nx = start_nx2;
		start_ny = start_ny2;
	}

	glPopMatrix();
}


void PhyBody :: drawBox (const float sides[3], const float color[4])
{	
	glShadeModel (GL_SMOOTH);

	//glPushMatrix();
	//glMultMatrixd(mf.matrix());
	glColor4f(color[0], color[1], color[2], color[3]);
	setTransform(pos, rot);

	float lx = sides[0]*0.5f;
	float ly = sides[1]*0.5f;
	float lz = sides[2]*0.5f;

	// sides
	glBegin (GL_TRIANGLE_STRIP);
	glNormal3f (-1,0,0);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,-ly,lz);
	glVertex3f (-lx,ly,-lz);
	glVertex3f (-lx,ly,lz);
	glNormal3f (0,1,0);
	glVertex3f (lx,ly,-lz);
	glVertex3f (lx,ly,lz);
	glNormal3f (1,0,0);
	glVertex3f (lx,-ly,-lz);
	glVertex3f (lx,-ly,lz);
	glNormal3f (0,-1,0);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,-ly,lz);
	glEnd();

	// top face
	glBegin (GL_TRIANGLE_FAN);
	glNormal3f (0,0,1);
	glVertex3f (-lx,-ly,lz);
	glVertex3f (lx,-ly,lz);
	glVertex3f (lx,ly,lz);
	glVertex3f (-lx,ly,lz);
	glEnd();

	// bottom face
	glBegin (GL_TRIANGLE_FAN);
	glNormal3f (0,0,-1);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,ly,-lz);
	glVertex3f (lx,ly,-lz);
	glVertex3f (lx,-ly,-lz);
	glEnd();
	glPopMatrix();
}

void PhyBody::setupDrawingMode(const float color[4])
{
	//glEnable (GL_LIGHTING);

	glDisable (GL_TEXTURE_2D);

	setColor (color[0],color[1],color[2],color[3]);

	if (color[3] < 1) 
	{
		glEnable (GL_BLEND);
		glBlendFunc (GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	}
	else
	{
		glDisable (GL_BLEND);
	}
}

void PhyBody::setColor (float r, float g, float b, float alpha)
{
	//GLfloat light_ambient[4],light_diffuse[4],light_specular[4];
	//light_ambient[0] = r*0.3f;
	//light_ambient[1] = g*0.3f;
	//light_ambient[2] = b*0.3f;
	//light_ambient[3] = alpha;
	//light_diffuse[0] = r*0.7f;
	//light_diffuse[1] = g*0.7f;
	//light_diffuse[2] = b*0.7f;
	//light_diffuse[3] = alpha;
	//light_specular[0] = r*0.2f;
	//light_specular[1] = g*0.2f;
	//light_specular[2] = b*0.2f;
	//light_specular[3] = alpha;
	//glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT, light_ambient);
	//glMaterialfv (GL_FRONT_AND_BACK, GL_DIFFUSE, light_diffuse);
	//glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, light_specular);
	//glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 2.0f);
}

void drawPatch (float p1[3], float p2[3], float p3[3], int level)
{
	int i;
	if (level > 0) {
		float q1[3],q2[3],q3[3];		 // sub-vertices
		for (i=0; i<3; i++) {
			q1[i] = 0.5f*(p1[i]+p2[i]);
			q2[i] = 0.5f*(p2[i]+p3[i]);
			q3[i] = 0.5f*(p3[i]+p1[i]);
		}
		float length1 = (float)(1.0/sqrt(q1[0]*q1[0]+q1[1]*q1[1]+q1[2]*q1[2]));
		float length2 = (float)(1.0/sqrt(q2[0]*q2[0]+q2[1]*q2[1]+q2[2]*q2[2]));
		float length3 = (float)(1.0/sqrt(q3[0]*q3[0]+q3[1]*q3[1]+q3[2]*q3[2]));
		for (i=0; i<3; i++) {
			q1[i] *= length1;
			q2[i] *= length2;
			q3[i] *= length3;
		}
		drawPatch (p1,q1,q3,level-1);
		drawPatch (q1,p2,q2,level-1);
		drawPatch (q1,q2,q3,level-1);
		drawPatch (q3,q2,p3,level-1);
	}
	else {
		glNormal3f (p1[0],p1[1],p1[2]);
		glVertex3f (p1[0],p1[1],p1[2]);
		glNormal3f (p2[0],p2[1],p2[2]);
		glVertex3f (p2[0],p2[1],p2[2]);
		glNormal3f (p3[0],p3[1],p3[2]);
		glVertex3f (p3[0],p3[1],p3[2]);
	}
}

void setTransform (const double pos[3], const double rot[12])
{
	GLdouble matrix[16];
	matrix[0]=rot[0];
	matrix[1]=rot[4];
	matrix[2]=rot[8];
	matrix[3]=0;
	matrix[4]=rot[1];
	matrix[5]=rot[5];
	matrix[6]=rot[9];
	matrix[7]=0;
	matrix[8]=rot[2];
	matrix[9]=rot[6];
	matrix[10]=rot[10];
	matrix[11]=0;
	matrix[12]=pos[0];
	matrix[13]=pos[1];
	matrix[14]=pos[2];
	matrix[15]=1;
	glPushMatrix();
	glMultMatrixd(matrix);
}
